/*
____/\\\\\\\\\_____        __/\\\________/\\\_        __/\\\\\\\\\\\_
 __/\\\///////\\\___        _\/\\\_______\/\\\_        _\/////\\\///__
  _\/\\\_____\/\\\___        _\/\\\_______\/\\\_        _____\/\\\_____
   _\/\\\\\\\\\\\/____        _\/\\\_______\/\\\_        _____\/\\\_____
    _\/\\\//////\\\____        _\/\\\_______\/\\\_        _____\/\\\_____
     _\/\\\____\//\\\___        _\/\\\_______\/\\\_        _____\/\\\_____
      _\/\\\_____\//\\\__        _\//\\\______/\\\__        _____\/\\\_____
       _\/\\\______\//\\\_        __\///\\\\\\\\\/___        __/\\\\\\\\\\\_
        _\///________\///__        ____\/////////_____        _\///////////__
*/
#include "../inc/RUI_MAIN.h"

float tmp = 0;

int main()
{
    while (1)
    {


        /* Example 1: PUMA560 ------------------------------------------------------------------------*/
//        float m[6] = {0.2645, 0.17, 0.1705, 0, 0, 0};
//
//        float tmp1[18] = {0, -8.5e-2, 0, 0, 0, 0,
//                          13.225e-2, 0, 0, 0, 0, 0,
//                          0, 3.7e-2, 8.525e-2, 0, 0, 0};
//        Matrixf<3, 6> rc(tmp1);
//        Matrixf<3, 3> I[6];
//
//        float tmp2[3] = {1.542e-3, 0, 1.542e-3};
//        float tmp3[3] = {0, 0.409e-3, 0.409e-3};
//        float tmp4[3] = {0.413e-3, 0.413e-3, 0};
//        I[0] = matrixf::diag<3, 3>(tmp2);
//        I[1] = matrixf::diag<3, 3>(tmp3);
//        I[2] = matrixf::diag<3, 3>(tmp4);
//        I[3] = matrixf::eye<3, 3>() * 3.0f;
//        I[4] = matrixf::eye<3, 3>() * 2.0f;
//        I[5] = matrixf::eye<3, 3>() * 1.0f;
//
//        robotics::Link links[6];
//        links[0] = robotics::Link(0, 26.45e-2, 0, -PI / 2, robotics::R, 0, 0, 0, m[0], rc.col(0), I[0]);
//        links[1] = robotics::Link(0, 5.5e-2, 17e-2, 0, robotics::R, 0, 0, 0, m[1], rc.col(1), I[1]);
//        links[2] = robotics::Link(0, 0, 0, -PI / 2, robotics::R, 0, 0, 0, m[2], rc.col(2), I[2]);
//        links[3] = robotics::Link(0, 17.05e-2, 0, PI / 2, robotics::R, 0, 0, 0, m[3], rc.col(3), I[3]);
//        links[4] = robotics::Link(0, 0, 0, -PI / 2, robotics::R, 0, 0, 0, m[4], rc.col(4), I[4]);
//        links[5] = robotics::Link(0, 0, 0, 0, robotics::R, 0, 0, 0, m[5], rc.col(5), I[5]);
//        robotics::Serial_Link<6> p560(links);
//
//        float q[6] = {0.2, -0.5, -0.3, -0.6, 0.5, 0.2};
//        float qv[6] = {1, 0.5, -1, 0.3, 0, -1};
//        float qa[6] = {0.2, -0.3, 0.1, 0, -1, 0};
//        float he[6] = {1, 2, -3, -0.5, -2, 1};
//
//        Matrixf<4, 4> T = p560.fkine(q);
//        Matrixf<6, 6> J = p560.jacob(q);
//
//        float tmp5[6] = {0, 0, 0, 0, 0.1, 0};
//        Matrixf<6, 1> q_ikine = p560.ikine(T, Matrixf<6, 1>(tmp5));
//        Matrixf<6, 1> torq = p560.rne(q, qv, qa, he);



        /* Example 2: UR -----------------------------------------------------------------------------*/
//    robotics::Link links[6];
//    links[0] = robotics::Link(0, 0, 0, PI / 2, robotics::R, 0);
//    links[1] = robotics::Link(0, 0, 5, 0, robotics::R, PI / 2);
//    links[2] = robotics::Link(0, 0, 5, 0, robotics::R, -PI / 2);
//    links[3] = robotics::Link(0, 1, 0, -PI / 2, robotics::R, 0);
//    links[4] = robotics::Link(0, 1, 0, -PI / 2, robotics::R, -PI / 2);
//    links[5] = robotics::Link(0, 1, 0, 0, robotics::R, 0);
//    robotics::Serial_Link<6> UR(links);

//    float q[6] = {0.1, 0.2, 0.3, 0.4, 0.5, 0.6};
//    Matrixf<4, 4> T = UR.fkine(q);
//    Matrixf<6, 6> J = UR.jacob(q);
//    Matrixf<6, 1> q_ikine = UR.ikine(T);

        /* Example 3: SCARA --------------------------------------------------------------------------*/
    robotics::Link links[4];
    links[0] = robotics::Link(0, 5, 5, 0);
    links[1] = robotics::Link(0, 0, 5, 0);
    links[2] = robotics::Link(0, 0, 0, PI);
    links[3] = robotics::Link(0, 0, 0, 0, robotics::P);
    robotics::Serial_Link<4> SCARA(links);

    float q[4] = {0.2, 0.5, 0.3, 3};
    float tmp6[4] = {0, 0, 0, 0};
    q[1] += tmp;
    Matrixf<4, 4> T = SCARA.fkine(q);
    Matrixf<6, 4> J = SCARA.jacob(q);

    auto start = std::chrono::high_resolution_clock::now();
    tmp += 0.001;

    Matrixf<4, 1> q_ikine = SCARA.ikine(T, Matrixf<4, 1>(tmp6));

    auto stop = std::chrono::high_resolution_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start).count();

    printf("time = %ld \n", duration);
//     QR & SVD, unfinished in current version
//     QR<6, 4> J_qr(J);
//     Matrixf<6, 6> Q = J_qr.Q();
//     Matrixf<6, 4> R = J_qr.R();
//     SVD<6, 4> J_svd(J);

    }
//    return 0;
}